#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>

std::shared_ptr<rclcpp::Node> node_;
geometry_msgs::msg::Twist cmd_vel_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
int nCount = 0;

void lidarcb(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
    if (nCount>0)
    {
        nCount--;
        return;
    }
    int nNum = msg->ranges.size();
    int nMid = nNum / 2;
    float MidDist = msg->ranges[nMid];
    RCLCPP_INFO(node_->get_logger(), "ranges[%d]=%0.3f", nMid, MidDist);

    if (MidDist < 1.5)
    {
        cmd_vel_.angular.z = 0.3;
        cmd_vel_.linear.x = 0.1;
    } else
    {
        cmd_vel_.linear.x = 0.1;
        cmd_vel_.angular.z = 0.0;
    }
    nCount = 100;
    
    cmd_pub_->publish(cmd_vel_);
}

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);

    node_ = std::make_shared<rclcpp::Node>("lidar_behavior_node");
    cmd_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
    auto lidar_sub = node_->create_subscription<sensor_msgs::msg::LaserScan>(
        "/scan",
        10,
        lidarcb
    );

    rclcpp::spin(node_);
    rclcpp::shutdown();
    return 0;
}